Lab 3 - Struktury danych w planowaniu ruchu (cz. 2)
Metody i algorytmy planowania ruchu - laboratorium
Lab 3 - Struktury danych w planowaniu ruchu - dostęp do mapy wysokościowej i OctoMap z poziomu kodu
1. Wprowadzenie
Celem zajęć jest poznanie mechanizmów subskrybowania danych z topików, które przechowują informację o modelu otoczenia robota. Testowana będzie mapa wysokościowa i Octomap.
⚠️ Pamiętaj, aby w każdym nowym terminalu zanim rozpoczniesz pracę skonfigurować środowisko ROS komendą
source /opt/ros/humble/setup.bash
lubsource install/setup.bash
2. Mapy wysokościowe - elevation map (ROS 1 Noetic)
Na początek przejdziemy przez te same kroki co na poprzednich zajęciach w celu uruchomienia symulacji z mapą wysokościową.
Przykładowe tworzenie i wyświetlanie mapy wysokościowej pokazane zostanie na przykładzie paczki: https://github.com/ANYbotics/elevation_mapping
Paczka ta nie została jeszcze w pełni zaimplementowana w ROS 2, dlatego tę część instrukcji (pkt. 2.) wykonać należy w ROS 1 (w wersji Noetic). W tym celu proszę skorzystać z przygotowanego obrazu w dockerze (obraz ma już zainstalowane wymagane pakiety ROSa).
Jeśli korzystasz z komputera w sali lab. 321, użyj komendy
docker images
, aby sprawdzić, czy na liście jest obraz o
nazwie ros1_miapr
. Jeśli nie ma takiego obrazu, pobierz
plik tar z obrazem i go wczytaj:
mkdir -p ~/miapr && cd ~/miapr
pip install --upgrade --no-cache-dir gdown
test -f ros1_img.tar || python3 -m gdown "https://drive.google.com/uc?id=1xMOBmKESodcqaL6PYAT1zSdC5IGLToSP&confirm=t"
docker load -i ros1_img.tar
Później uruchom kontener poleceniem:
xhost + && docker run -it \
--env="DISPLAY" \
--env="QT_X11_NO_MITSHM=1" \
--volume="/tmp/.X11-unix:/tmp/.X11-unix:rw" \
--net=host \
--privileged \
--gpus=all \
--name=ros1_miapr_lab2 \
ros1_miapr:latest
Aby otworzyć kolejny terminal w dockerze, należy korzystać z polecenia:
docker exec -it ros1_miapr_lab2 bash
Poniższe komendy wykonuj w kontenerze. Pobierz biblioteki z repozytorium github:
cd /home/catkin_ws/src
git clone https://github.com/ANYbotics/elevation_mapping
git clone https://github.com/ANYbotics/kindr.git
git clone https://github.com/ANYbotics/kindr_ros.git
git clone https://github.com/ANYbotics/message_logger.git
git clone https://github.com/ANYbotics/point_cloud_io.git
Następnie skompiluj pobraną paczkę i odśwież przestrzeń roboczą:
cd /home/catkin_ws
source /opt/ros/noetic/setup.bash
catkin_make
source devel/setup.bash
Uruchom symulację poleceniem i czekaj (uruchomienie symulacji po raz pierwszy może zająć kilka minut):
roslaunch elevation_mapping_demos turtlesim3_waffle_demo.launch
Powinien pojawić się robot turtlebot3 waffle w środowisku Gazebo. Aby
sterować robotem z klawiatury, należy użyć modułu
turtlebot3_teleop
(w osobnym terminalu):
export TURTLEBOT3_MODEL=waffle
roslaunch turtlebot3_teleop turtlebot3_teleop_key.launch
Sterowanie robotem odbywa się za pomocą klawiszy ‘a’, ‘s’, ‘d’, ‘w’, ‘x’. Podczas poruszania się robota powinna budować się mapa wysokościowa terenu. Aktualizacja mapy może zająć około 60 s, dlatego należy poruszać się z niewielką prędkością.
2.1 Dostęp do komórek mapy wysokościowej z poziomu kodu
Uruchom symulację robota Turtlebot3 z mapą wysokościową:
roslaunch elevation_mapping_demos turtlesim3_waffle_demo.launch
Po uruchomieniu modułu do budowy mapy wysokościowej sprawdź format
danych zapisanych w topiku
/elevation_mapping/elevation_map_raw
poleceniem:
rostopic info /elevation_mapping/elevation_map_raw
Dokumentacja typu grid_map_msgs/GridMap
znajduje się pod
adresem: http://docs.ros.org/kinetic/api/grid_map_msgs/html/msg/GridMap.html
Dane dotyczące mapy przechowywane są w strukturze
std_msgs/Float32MultiArray
: http://docs.ros.org/kinetic/api/std_msgs/html/msg/Float32MultiArray.html
Struktura ta umożliwia przechowywanie wielowarstwowych tablic
wielowymiarowych. W przypadku mapy wysokościowej mamy dwie warstwy -
elevation
i variance
. Indeksowanie danych jest
nietypowe i pokazane zostało na przykładzie zadania poniżej.
🔨 Zadanie 2.1.1
W katalogu catkin_ws/src
utwórz nową paczkę o nazwie
elevation_map_io
:
catkin_create_pkg elevation_map_io rospy grid_map_msgs
Następnie utwórz w niej katalog scripts
, a w nim stwórz
plik invert_map.py
i dodaj mu prawa do wykonywania.
Skompiluj workspace.
Napisz węzeł (w utworzonym skrypcie), który zmieni znak wysokości
każdej z komórek oryginalnej mapy i wynik opublikuj w nowej mapie.
Ponieważ mapa wysokościowa może działać stosunkowo wolno, poniżej podane
jest rozwiązanie. Uruchom przykład poleceniem rosrun
i
wyświetl uzyskaną mapę (topik /map_copy
):
#!/usr/bin/env python3
import rospy
from grid_map_msgs import msg # import occupancy grid data
= msg.GridMap()
map_cpy
def callback(elev_map):
#print available layers
for n in elev_map.basic_layers:
print(n)
# access the first layer (index 0) - elevation
= elev_map.data[0].layout.dim[0].stride
stride0 = elev_map.data[0].layout.dim[1].stride
stride1 # dimension
= elev_map.data[0].layout.dim[0].size
cols = elev_map.data[0].layout.dim[1].size
rows # offset
= elev_map.data[0].layout.data_offset
offset = elev_map.data
map_cpy.data # create list to edit tuple
= list(map_cpy.data[0].data)
data_tmp for i in range(cols):
for j in range(rows):
+ i + stride1 * j + 0] = -elev_map.data[0].data[offset + i + stride1 * j + 0]
data_tmp[offset = elev_map.info
map_cpy.info = elev_map.layers
map_cpy.layers = elev_map.basic_layers
map_cpy.basic_layers = elev_map.outer_start_index
map_cpy.outer_start_index = elev_map.inner_start_index
map_cpy.inner_start_index # copy to tuple
0].data = tuple(data_tmp)
map_cpy.data[
def mapListener():
#create node
'map_listener', anonymous=True)
rospy.init_node(#subscribe /map topic
"/elevation_mapping/elevation_map_raw", msg.GridMap, callback)
rospy.Subscriber(# spin() simply keeps python from exiting until this node is stopped
= rospy.Publisher('map_copy', msg.GridMap, queue_size=10)
pub
= rospy.Rate(1) # 1hz
rate while not rospy.is_shutdown():
#publish copy of the map
pub.publish(map_cpy)
rate.sleep()
if __name__ == '__main__':
mapListener()
3. Dostęp do komórek mapy OctoMap (ROS 2 Humble)
Zakończ działanie wszystkich węzłów z poprzedniego zadania. W dalszej części instrukcji będziemy pracować na ROS 2 (bez dockera). Aby uruchomić bibliotekę Octomap, należy ją wcześniej zainstalować z repozytorium:
sudo apt-get install ros-humble-octomap ros-humble-octomap-server python3-vcstool ros-humble-turtlebot3* ros-humble-sensor-msgs-py
Następnie pobierz repozytorium zawierające skrypty uruchomieniowe oraz przykładowe mapy:
cd ~/ros2_ws/src
git clone --branch humble https://github.com/dominikbelter/example_maps
git clone https://github.com/iKrishneel/octomap_server2.git
cd octomap_server2 && vcs import . < deps.repos
Skompiluj wszystkie paczki znajdujące się w przestrzeni roboczej:
cd ~/ros2_ws/
colcon build --symlink-install
⚠️ W przypadku błędu w kompilacji:
fatal error: message_filters/message_event.hpp: No such file or directory 44 | #include <message_filters/message_event.hpp>
należy przejść do katalogu
/home/student/ros2_ws/src/perception_pcl
i cofnąć się do jednego z wcześniejszych commitów:git checkout -b new_branch 44d02743e451296d6bc871b101cea33c59adc1d6
Jeżeli kompilacja zakończyła się sukcesem, należy zmodyfikować model robota Turtlebot3, tak aby korzystał z kamery głębi. W tym celu otworzyć plik:
sudo gedit /opt/ros/humble/share/turtlebot3_gazebo/models/turtlebot3_waffle/model.sdf
I dokonać zmian w liniach:
line 369: <sensor name="camera" type="depth">
line 400: <frame_name>camera_rgb_optical_frame</frame_name>
Teraz można uruchomić symulację robota Turtlebot3 wraz z systemem do budowy mapy:
source install/setup.bash
export TURTLEBOT3_MODEL=waffle
ros2 launch example_maps turtlesim3_waffle_octomap.launch.py
Do sterowania robotem wykorzystamy moduł
turtlebot3_teleop
(w osobnym terminalu):
cd ~/ros2_ws
source install/setup.bash
export TURTLEBOT3_MODEL=waffle
ros2 run turtlebot3_teleop teleop_keyboard
Aby dostać się do danych przechowywanych w mapie OctoMap,
wykorzystamy publikowany przez ten moduł topik o nazwie
/octomap_point_cloud_centers
. W tym topiku przechowywane są
środki wokseli, które są zajęte. Sprawdź typ danych poleceniem:
ros2 topic info /octomap_point_cloud_centers
Typ sensor_msgs/PointCloud2
opisany jest pod adresem: https://docs.ros2.org/galactic/api/sensor_msgs/msg/PointCloud2.html
🔨 Zadanie 3.1
Utwórz paczkę w katalogu ~/ros2_ws/src
o nazwie
octomap_manipulation
(analogicznie jak na poprzednich
zajęciach, do dependencies
dodaj sensor_msgs
).
Wykorzystując poniższy szablon projektu, napisz węzeł odczytujący i
wyświetlający w terminalu współrzędne zajętych voxeli:
import rclpy
from rclpy.node import Node
import numpy as np
from sensor_msgs.msg import PointCloud2
from sensor_msgs_py import point_cloud2
class OctomapSubscriber(Node):
def __init__(self):
super().__init__('octomap_subscriber')
self.subscription = self.create_subscription(
PointCloud2,'octomap_point_cloud_centers',
self.listener_callback,
10)
self.subscription # prevent unused variable warning
def listener_callback(self, cloud_msg):
self.get_logger().info('I heard a pointcloud')
= point_cloud2.read_points(cloud_msg)
gen print(type(gen))
# TODO: print x y z
def main(args=None):
=args)
rclpy.init(args= OctomapSubscriber()
octomap_subscriber
rclpy.spin(octomap_subscriber)
octomap_subscriber.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
4. Wyświetlanie zadanej ścieżki w RViz
Podczas planowania ruchu przydatne jest wyświetlanie zaplanowanej
ścieżki. Wykorzystamy do tego typ nav_msgs/Path
: https://docs.ros2.org/foxy/api/nav_msgs/msg/Path.html
Uruchom przykład poniżej i wyświetl ścieżkę z topiku
/my_path
w RViz (wpisz odpowiedni frame_id
w
Global Options -> Fixed Frame).
import rclpy
from rclpy.node import Node
from nav_msgs.msg import Path
from geometry_msgs.msg import PoseStamped
import numpy as np
class PathPublisher(Node):
def __init__(self):
super().__init__('path_publisher')
self.publisher_ = self.create_publisher(Path, 'my_path', 10)
= 0.5 # seconds
timer_period self.timer = self.create_timer(timer_period, self.timer_callback)
def timer_callback(self):
= Path()
path = 'odom'
path.header.frame_id
# create path
= PoseStamped()
pose1 = 0.0
pose1.pose.position.x = 0.0
pose1.pose.position.y = 0.0
pose1.pose.position.z #orientation defined as a quaternion
= 0.0
pose1.pose.orientation.x = 0.0
pose1.pose.orientation.y = 0.0
pose1.pose.orientation.z = 1.0
pose1.pose.orientation.w = 'odom'
pose1.header.frame_id = self.get_clock().now().to_msg()
pose1.header.stamp
path.poses.append(pose1)
= PoseStamped()
pose2 = -1.0
pose2.pose.position.x = -1.0
pose2.pose.position.y = 1.0
pose2.pose.position.z # orientation defined as a quaternion
= 0.0
pose2.pose.orientation.x = 0.0
pose2.pose.orientation.y = 0.0
pose2.pose.orientation.z = 1.0
pose2.pose.orientation.w = 'odom'
pose2.header.frame_id = self.get_clock().now().to_msg()
pose2.header.stamp
path.poses.append(pose2)
#publish path
self.publisher_.publish(path)
self.get_logger().info('Publishing path')
def main(args=None):
=args)
rclpy.init(args= PathPublisher()
path_publisher
rclpy.spin(path_publisher)
path_publisher.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
🔨 Zadanie 4.1
Zmodyfikuj przykład tak, aby w RViz wyświetlał się okrąg.